Self-Adaptive Horizontal Attitude Measurement Method based on Motion State Monitoring

ABSTRACT

Disclosed is a self-adaptive horizontal attitude measurement method based on motion state monitoring. Based on a newly established state space model, a horizontal attitude angle is taken as a state variable, an angular velocity increment Δω b  for compensating a random constant zero offset is taken as a control input of a system equation, and a specific force f b  for compensating the random constant zero offset is taken as a measurement quantity. Meanwhile, judgment conditions for a maneuvering state of a carrier are improved, and maneuvering information of the carrier is judged by comprehensively utilizing acceleration information and angular velocity information output by a micro electro mechanical system inertial measurement unit (MEMS-IMU), whereby a measurement noise matrix of a filter can be automatically adjusted, thereby effectively reducing the influence of carrier maneuvering on the calculation of a horizontal attitude. The method has no special requirement on the maneuvering state of the carrier, and can ensure that the system has high attitude measurement precision in different motion states without an external information assistance.

TECHNICAL FIELD

The disclosure relates to a self-adaptive horizontal attitudemeasurement method based on motion state monitoring, and belongs to thetechnical field of inertia.

BACKGROUND

With the development of micro electro mechanical system technology, alow-cost micro electro mechanical system inertial measurement unit(MEMS-IMU) is more frequently applied in the field of navigation. Bymeasuring motion parameters by using inertial sensors based on a microelectro mechanical system, a complex motion state of a vessel in the seacan be detected, and users can acquire motion data of a surface vessel.The MEMS-IMU is required to accurately output angular motion parametersand linear motion parameters of a carrier in real time.

Micro electro mechanical gyroscopes have random drift characteristics,and integral errors thereof are accumulated over time, whileaccelerometers do not have accumulated errors but are susceptible tocarrier vibration. Common algorithms for fusing data of the gyroscopesand the accelerometers are Kalman filtering and complementary filtering.For example, in the patent application No. 201811070907.X, entitled“Horizontal Attitude Self-Correction Method for MEMS Inertial NavigationSystem based on Maneuvering State Judgment”, a carrier motion is dividedinto low, medium, and high dynamics by comparing an accelerometer outputand a local gravity acceleration amplitude. In the low and mediumdynamics, a measurement noise matrix is adjusted in real time. In thehigh dynamics, only time is updated. However, if the carrier is in thehigh dynamics for a long time, an attitude error will be increasing. Foranother example, in the patent application No. 202011092956.0, entitled“MARG Attitude Estimation Method for Small Unmanned Aerial Vehicle basedon Self-Adaptive EKF Algorithm”, an adaptive filtering algorithm for anexternal acceleration analyzes residual errors of three axes and thenself-adaptively adjusts corresponding measurement noises, therebyavoiding losing useful acceleration information and improving theprecision of attitude estimation. According to most of the traditionalmethods of self-adaptive adjustment, by comparing a modulus output by anaccelerometer of a carrier with a local gravity acceleration, amaneuvering state of the carrier can be judged without considering theinterference of an angular motion of the carrier on accelerationmeasurement. It may be insufficient for some complex environments.

Summary

In view of the prior art, the technical problem to be solved by thedisclosure is to provide a self-adaptive horizontal attitude measurementmethod based on motion state monitoring, which can improve themeasurement precision of a horizontal attitude of a carrier in amaneuvering scene and provide accurate horizontal attitude informationfor the carrier.

In order to solve the above technical problem, a self-adaptivehorizontal attitude measurement method based on motion state monitoringof the disclosure includes the following steps:

step 1: initially aligning a strapdown inertial navigation system,completing the calculation of a random constant zero offset of a deviceand the calculation of initial horizontal attitude angles, including aroll angle

₀ and a pitch angle ϕ₀, and then entering a navigation working mode;

step 2: initializing a Kalman filter, and taking the initial horizontalattitude angles

₀ and ϕ₀ obtained in step 1 as initial values of a Kalman filteringstate quantity X₀=[

₀ ϕ₀]^(T), an initial mean square error being P₀;

step 3: sampling MEMS-IMU data at a k^(th) time, and compensating arandom constant zero offset thereof to obtain a compensated specificforce f_(k) ^(b) and angular velocity ω_(k) ^(b);

step 4: performing a Kalman filtering one-step prediction by using anangular velocity increment Δω_(k) ^(b) at the k^(th) time as a knowndeterministic input u_(k−1), where Δω_(k) ^(b)=ω_(k) ^(b)·T, T being acalculation period;

step 5: judging a maneuvering state of a carrier by using the specificforce f^(b) and the angular velocity ω^(b) from time k−N+1 to time kobtained in step 3, and self-adaptively adjusting a Kalman filteringmeasurement noise covariance matrix R_(k), where N is the size of a datawindow;

step 6: when the specific force is f_(k) ^(b)=[f_(x,k) f_(y,k)f_(z,k)]^(T) at the k^(th) time, selecting a measurement vectorZ_(k)=[f_(x,k) f_(y,k) ]^(T) to perform measurement update so as torealize the correction of the state quantity, where f_(x,k), f_(y,k),and f_(z,k) are components of the specific force f_(k) ^(b) n x-axis,y-axis, and z-axis directions of a carrier system respectively; and

step 7: taking an estimated value of the state quantity at the k^(th)time as an initial value of a state quantity at a next time, andrepeatedly performing steps 3 to 6 until a navigation working stateends.

The method of the disclosure further includes the following operations:

1. The Kalman filtering one-step prediction in step 4 specifically is:

$\left\{ \begin{matrix}{{\hat{X}}_{{k/k} - 1} = {{\Phi_{{k/k} - 1}{\hat{X}}_{k - 1}} + {B_{k - 1}u_{k - 1}}}} \\{P_{{k/k} - 1} = {{\Phi_{{k/k} - 1}P_{k - 1}\Phi_{{k/k} - 1}^{T}} + Q_{k - 1}}}\end{matrix} \right.$

where {circumflex over (X)}_(k−1) is a state optimal estimation at ak−1^(th) time, P_(k−1) is a mean square error matrix of a stateestimation at the k−1^(th) time, {circumflex over (X)}_(k/k−1) is astate one-step prediction at a k^(th) time, Φ_(k/k−1)=I₂ is a stateone-step transition matrix at the k^(th) time, I₂ is a second-order unitmatrix,

B k = [ 0 cos ⁢ ϕ k - 1 - sin ⁢ ϕ k - 1 1 sin ⁢ ϕ k - 1 ⁢ tan k - 1 cos ⁢ ϕk - 1 ⁢ tan k - 1 ]

is an input coefficient matrix at the k^(th) time, ϕ_(k−1) and

_(k−1) are horizontal attitude optimal estimations at the k−1^(th) time,u_(k−1) is an angular velocity increment Δω_(k) ^(b) at the k^(th) time,P_(k/k−1) is a state one-step prediction mean square error matrix at thek^(th) time, and Q_(k) is a system noise variance matrix.

2. In step 5, the judging a maneuvering state of a carrier by using thespecific force f^(b) and the angular velocity ω^(b) obtained in step 3specifically is: calculating the maneuvering state of the carrier byusing the specific force f^(b) and the angular velocity ω^(b) from timek−N+1 to time k obtained in step 3, and acquiring a maneuvering vectorT_(k) to realize maneuvering judgment, T_(k) satisfying:

$T_{k} = \left( {{\frac{1}{\sigma_{f}^{2}}{{f_{k}^{b} - {g\frac{{\overset{\_}{f}}_{k}}{{\overset{\_}{f}}_{k}}}}}} + {\frac{1}{\sigma_{\omega}^{2}}{{\overset{\_}{\omega}}_{k}}}} \right)$${{where}{\overset{\_}{\omega}}_{k}} = {{\frac{1}{N}{\sum\limits_{i = {k - N + 1}}^{k}{\omega_{i}^{b}{and}{\overset{\_}{f}}_{k}}}} = {\frac{1}{N}{\sum\limits_{i = {k - N + 1}}^{k}f_{i}^{b}}}}$

are mean values obtained by performing moving average on the specificforce f^(b) and the angular velocity ω^(b), which are output by aninertial measurement unit and used for compensating the random constantzero offset, from time k−N+1 to time k in step 3, respectively, the sizeof a data window being N, g being a local gravitational acceleration,and σ_(f) and σ_(ω) being weighting coefficients respectively.

3. The self-adaptively adjusting a Kalman filtering measurement noisecovariance matrix R_(k) in step 5 specifically is:

$R_{k} = \begin{bmatrix}{\sigma_{r}^{2} + T_{k,1}} & 0 \\0 & {\sigma_{r}^{2} + T_{k,2}}\end{bmatrix}$

where T_(k,i) is an i^(th) element of T_(k) , and σ_(r) ² is a variancecorresponding to an accelerometer noise.

4. An update equation of the measurement update in step 6 is:

{ K k = P k / k - 1 ⁢ H k T ( H k ⁢ P k / k - 1 ⁢ H k T + R k ) - 1 X ^ k =X ^ k / k - 1 + K k ( Z k - H k ⁢ X ^ k / k - 1 ) P k = ( I - K k ⁢ H k ) ⁢P k / k - 1 ⁢ where ⁢ H k = [ - g · cos k 0 - g · sin ⁢ ϕ k ⁢ cos k g · cos ⁢ϕ k ⁢ cos k ]

is a measurement matrix at time k, g is a local gravity acceleration,ϕ_(k) and

_(k) are one-step prediction values of a horizontal attitude at time k,K_(k) is a filtering gain at time k, {circumflex over (X)}_(k) is astate estimation at time k, and P_(k) is a state estimation mean squareerror matrix at time k.

The disclosure has the following beneficial effects. The disclosurerelates to an attitude measurement unit using an MEMS-IMU as a coredevice. In the disclosure, a new state space model is established, ahorizontal attitude angle is taken as a state variable, an angularvelocity increment Δω^(b) output by the MEMS-IMU is taken as a controlinput of a system equation, and a specific force f^(b) output by theMEMS-IMU is taken as a measurement quantity. Meanwhile, maneuveringjudgment conditions for a carrier are improved, and maneuveringinformation of the carrier is judged by comprehensively utilizingacceleration information and angular velocity information output by theMEMS-IMU, whereby a measurement noise matrix of a filter can beautomatically adjusted, thereby effectively reducing the influence ofcarrier maneuvering on horizontal attitude information. The method hasno special requirement on the maneuvering state of the carrier, canensure that the system has high attitude measurement precision indifferent motion states without an external information assistance, andhas a certain engineering application value.

BRIEF DESCRIPTION OF FIGURES

FIG. 1 is an implementation flowchart of the disclosure.

FIG. 2 is a flowchart of an implementation algorithm of the disclosure.

FIG. 3 is an algorithm calculation error according to the disclosure.

DETAILED DESCRIPTION

The disclosure will now be further described with reference to theaccompanying drawings and specific embodiments.

The disclosure is implemented as follows:

An inertial measurement element of a strapdown inertial navigationsystem is fully pre-heated, and the calculation of a random constantzero offset of a device and the calculation of initial horizontalattitude angles are completed. Then a navigation working state may beentered. A horizontal attitude angle is taken as a state variable, anangular velocity increment Δω^(b) output by an MEMS-IMU is taken as acontrol input of a system equation, and a specific force f^(b) output bythe MEMS-IMU is taken as a measurement quantity. A Kalman filteringequation is established, and maneuvering information of the carrier isjudged by comprehensively utilizing acceleration information and angularvelocity information output by the MEMS-IMU, whereby a measurement noisematrix of a filter can be automatically adjusted, thereby effectivelyreducing the influence of carrier maneuvering on the calculation of ahorizontal attitude. The specific steps are as follows:

In step 1, an inertial measurement element of a strapdown inertialnavigation system is fully pre-heated, the calculation of a randomconstant zero offset of a device and the calculation of initialhorizontal attitude angles (a roll angle

₀ and a pitch angle ϕ₀) are completed, and the element enters anavigation working state.

In step 2, the initial horizontal attitude angles

₀ and ϕ₀ obtained in step 1 are taken as initial values of a Kalmanfiltering state quantity X₀=[

₀ ϕ₀]^(T), an initial mean square error is P₀, and a Kalman filter isinitialized.

In step 3, MEMS-IMU data is sampled at a k^(th) time, and a randomconstant zero offset thereof is compensated to obtain a compensatedspecific force f_(k) ^(b) and angular velocity ω_(k) ^(b).

In step 4, a Kalman filtering one-step prediction is performed by usingan angular velocity increment Δω_(k) ^(b) at the k^(th) time as a knowndeterministic input u_(k−1), where Δω_(k) ^(b)=ω_(k) ^(b)·T, T being acalculation period.

In step 5, a maneuvering state of a carrier is judged by using thespecific force f^(b) and the angular velocity ω^(b) from time k−N+1 totime k obtained in step 2, and a Kalman filtering measurement noisecovariance matrix R_(k) is self-adaptively adjusted, where N is the sizeof a data window.

In step 6, the specific force f_(k) ^(b) at the k^(th) time is used as ameasurement vector Z_(k) for measurement update so as to realize thecorrection of the state quantity.

In step 7, an estimated value of the state quantity at the k^(th) timeis taken as an initial value of a state quantity at a next time, andsteps 3 to 6 are repeatedly performed until a navigation working stateends.

A correlated equation for Kalman filtering one-step prediction in step 4is established:

$\left\{ \begin{matrix}{{\hat{X}}_{{k/k} - 1} = {{\Phi_{{k/k} - 1}{\hat{X}}_{k - 1}} + {B_{k - 1}u_{k - 1}}}} \\{P_{{k/k} - 1} = {{\Phi_{{k/k} - 1}P_{k - 1}\Phi_{{k/k} - 1}^{T}} + Q_{k - 1}}}\end{matrix} \right.$

where {circumflex over (X)}_(k−1) is a state optimal estimation at ak−1^(th) time, P_(k−1) is a mean square error matrix of a stateestimation at the k−1^(th) time, {circumflex over (X)}_(k/k−1) is astate one-step prediction at a k^(th) time, Φ_(k/k−1)=I₂ is a stateone-step transition matrix at the k^(th) time, I₂ is a second-order unitmatrix,

$B_{k} = \begin{bmatrix}0 & {\cos\phi_{k - 1}} & {{- \sin}\phi_{k - 1}} \\1 & {\sin\phi_{k - 1}\tan\vartheta_{k - 1}} & {\cos\phi_{k - 1}\tan\vartheta_{k - 1}}\end{bmatrix}$

is an input coefficient matrix at the k^(th) time, ϕ_(k−1) and

_(k−1) are horizontal attitude optimal estimations at the k−1^(th) time,u_(k−1) is an angular velocity increment Δω_(k) ^(b) at the k^(th) time,P_(k/k−1) is a state one-step prediction mean square error matrix at thek^(th) time, and Q_(k) is a system noise variance matrix.

In step 5, the maneuvering state of the carrier is judged, themaneuvering state of the carrier is calculated according to the specificforce f^(b) and the angular velocity ω^(b) obtained in step 3, and amaneuvering vector T_(k) is acquired to realize maneuvering judgment:

$T_{k} = \left( {{\frac{1}{\sigma_{f}^{2}}{{f_{k}^{b} - {g\frac{{\overset{¯}{f}}_{k}}{{\overset{¯}{f}}_{k}}}}}} + {\frac{1}{\sigma_{\omega}^{2}}{{\overset{\_}{\omega}}_{k}}}} \right)$${{where}{\overset{¯}{\omega}}_{k}} = {{\frac{1}{N}{\sum\limits_{i = {k - N + 1}}^{k}{\omega_{i}^{b}{and}{\overset{¯}{f}}_{k}}}} = {\frac{1}{N}{\sum\limits_{i = {k - N + 1}}^{k}f_{i}^{b}}}}$

are mean values obtained by performing moving average on the specificforce f^(b) and the angular velocity ω^(b) from time k−N+1 to time k instep 3, respectively, the size of a data window being N, g being a localgravitational acceleration, and σ_(f) and σ_(ω) are weightingcoefficients respectively.

A self-adaptive rule for the Kalman filtering measurement noisecovariance matrix R_(k) in step 5 is as follows:

$R_{k} = \begin{bmatrix}{\sigma_{r}^{2} + T_{k,1}} & 0 \\0 & {\sigma_{r}^{2} + T_{k,2}}\end{bmatrix}$

where T_(k,i) is an i^(th) element of T_(k), and σ_(r) ² is a variancecorresponding to an accelerometer.

In step 6, measurement update is performed. The specific force f_(k)^(b)=[f_(x,k) f_(y,k) f_(z,k)]^(T) at the k^(th) time in step 3 is usedas a measurement vector Z_(k)=[f_(x,k) f_(y,k)]^(T) for measurementupdate. An update equation is as follows:

$\left\{ {{\begin{matrix}{K_{k} = {P_{{k/k} - 1}{H_{k}^{T}\left( {{H_{k}P_{{k/k} - 1}H_{k}^{T}} + R_{k}} \right)}^{- 1}}} \\{{\overset{\hat{}}{X}}_{k} = {{\overset{\hat{}}{X}}_{{k/k} - 1} + {K_{k}\left( {Z_{k} - {H_{k}{\overset{\hat{}}{X}}_{{k/k} - 1}}} \right)}}} \\{P_{k} = {\left( {I - {K_{k}H_{k}}} \right)P_{{k/k} - 1}}}\end{matrix}{where}H_{k}} = \begin{Bmatrix}{{{- g} \cdot \cos}\vartheta_{k}} & 0 \\{{{- g} \cdot \sin}\phi_{k}\cos\vartheta_{k}} & {{g \cdot \cos}\phi_{k}\cos\vartheta_{k}}\end{Bmatrix}} \right.$

is a measurement matrix at time k, ϕ_(k) and

_(k) are one-step prediction values of a horizontal attitude at time k,K_(k) is a filtering gain at time k, {circumflex over (X)}_(k) is astate estimation at time k, and P_(k) is a state estimation mean squareerror matrix at time k.

With reference to FIGS. 1 and 2, a specific embodiment of the disclosureincludes the following steps:

In step 1, an inertial measurement element of a strapdown inertialnavigation system is fully pre-heated.

In step 2, data of MEMS-IMU under a static state during a period of timeis acquired, and it is considered that an average output value of anMEMS-IMU during the period of time is a random constant zero offset of adevice, constant zero offset errors for a gyroscope and an accelerometerare corrected and compensated, and a specific force f^(b) and an angularvelocity ω^(b) after compensating the constant zero offset errors areobtained.

In step 3, the strapdown inertial navigation system is initially alignedto obtain initial horizontal attitude angles (a roll angle

₀ and a pitch angle ϕ₀) of a carrier system (b system) relative to anavigation coordinate system (n system, the navigation coordinate systemherein being a geographical coordinate system), and then a navigationworking state starts to be entered.

In step 4, a Kalman filtering equation is established:

$\begin{matrix}\left\{ \begin{matrix}{X_{k} = {{\Phi_{{k/k} - 1}X_{k - 1}} + {B_{k - 1}u_{k - 1}} + W_{k - 1}}} \\{Z_{k} = {{H_{k}X_{k}} + V_{k}}}\end{matrix} \right. & (1)\end{matrix}$

where X_(k) is a state vector, Φ_(k/k−1) is a state one-step transitionmatrix, B_(k−1) is an input coefficient matrix, I₂ is a second-orderunit matrix, u_(k−1) is a known deterministic input, Z_(k) is ameasurement vector, H_(k) is a measurement matrix, W_(k−1) is a systemnoise vector, and V_(k) is a measurement noise vector.

In step 5, horizontal attitude angles are selected as a state quantityX=[

ϕ]^(T) (a roll angle

and a pitch angle ϕ), the initial horizontal attitude angles

₀ and ϕ₀ obtained in step 3 are taken as initial values of the statequantity X₀=[

₀ ϕ₀]^(T), an initial mean square error is P₀, and a Kalman filter isinitialized.

In step 6, MEMS-IMU data is sampled at a k^(th) time, and a randomconstant zero offset thereof is compensated to obtain a compensatedspecific force f_(k) ^(b) and angular velocity ω_(k) ^(b).

In step 7, a Kalman filtering one-step prediction is performed by usingan increment Δω_(k) ^(b)=ω_(k) ^(b)·T (T being a calculation period)within a ω_(k) ^(b) sampling interval at the k^(th) time in step 6 as aknown deterministic input u_(k−1):

$\begin{matrix}\left\{ \begin{matrix}{{\overset{\hat{}}{X}}_{{k/k} - 1} = {{\Phi_{{k/k} - 1}{\overset{\hat{}}{X}}_{k - 1}} + {B_{k - 1}u_{k - 1}}}} \\{P_{{k/k} - 1} = {{\Phi_{{k/k} - 1}P_{k - 1}\Phi_{{k/k} - 1}^{T}} + Q_{k - 1}}}\end{matrix} \right. & (2)\end{matrix}$

where {circumflex over (X)}_(k−1) is a state optimal estimation at ak−1^(th) time, P_(k−1) is a mean square error matrix of a stateestimation at the k−1^(th) time, {circumflex over (X)}_(k/k−1) is astate one-step prediction at a k_(th) time, Φ_(k/k−1)=I₂ is a stateone-step transition matrix at the k^(th) time,

$B_{k} = \begin{bmatrix}0 & {\cos\phi_{k - 1}} & {{- \sin}\phi_{k - 1}} \\1 & {\sin\phi_{k - 1}\tan\vartheta_{k - 1}} & {\cos\phi_{k - 1}\tan\vartheta_{k - 1}}\end{bmatrix}$

is an input coefficient matrix at the k^(th) time, ϕ_(k−1) and

_(k−1) are horizontal attitude optimal estimations at the k−1^(th) time,u_(k−1) is an angular velocity increment Δω_(k) ^(b) at the k^(th) time,P_(k/k−1) is a state one-step prediction mean square error matrix at thek^(th) time, and Q_(k) is a system noise variance matrix.

In step 8, a maneuvering state of a carrier is judged by using thespecific force f^(b) and the angular velocity ω^(b) from time k−N+1 totime k obtained in step 6, and a maneuvering vector T_(k) is acquired,thereby self-adaptively adjusting a Kalman filtering measurement noisecovariance matrix R_(k).

$\begin{matrix}{T_{k} = \left( {{\frac{1}{\sigma_{f}^{2}}{{f_{k}^{b} - {g\frac{{\overset{¯}{f}}_{k}}{{\overset{¯}{f}}_{k}}}}}} + {\frac{1}{\sigma_{\omega}^{2}}{{\overset{\_}{\omega}}_{k}}}} \right)} & (3)\end{matrix}$${{where}{\overset{¯}{\omega}}_{k}} = {{\frac{1}{N}{\sum\limits_{i = {k - N + 1}}^{k}{\omega_{i}^{b}{and}{\overset{¯}{f}}_{k}}}} = {\frac{1}{N}{\sum\limits_{i = {k - N + 1}}^{k}f_{i}^{b}}}}$

are mean values obtained by performing moving average on the specificforce f^(b) and the angular velocity ω^(b) from time k−N+1 to time k instep 6, respectively, the size of a data window is N, g is a localgravitational acceleration, σ_(f) is a weighting coefficient of thespecific force (usually 0.5-5 for vessels and 1-3 for vehicles), andσ_(ω) is a weighting coefficient of the angular velocity (usually 1-5for vessels and 0.5-1 for vehicles).

A self-adaptive adjustment rule for the Kalman filtering measurementnoise covariance matrix R_(k) is as follows:

$\begin{matrix}{R_{k} = \begin{bmatrix}{\sigma_{r}^{2} + T_{k,1}} & 0 \\0 & {\sigma_{r}^{2} + T_{k,2}}\end{bmatrix}} & (4)\end{matrix}$

where T_(k,i) is an i^(th) element of T_(k), and σ_(r) ² is a variancecorresponding to an accelerometer.

In step 9, the specific force f_(k) ^(b)=[f_(x,k) f_(y,k) f_(z,k)]^(T)at the k^(th) time in step 6 is used as a measurement vectorZ_(k)=[f_(x,k) f_(y,k) ]^(T) for measurement update. An update equationis as follows:

$\begin{matrix}\left\{ {{\begin{matrix}{K_{k} = {P_{{k/k} - 1}{H_{k}^{T}\left( {{H_{k}P_{{k/k} - 1}H_{k}^{T}} + R_{k}} \right)}^{- 1}}} \\{{\overset{\hat{}}{X}}_{k} = {{\overset{\hat{}}{X}}_{{k/k} - 1} + {K_{k}\left( {Z_{k} - {H_{k}{\overset{\hat{}}{X}}_{{k/k} - 1}}} \right)}}} \\{P_{k} = {\left( {I - {K_{k}H_{k}}} \right)P_{{k/k} - 1}}}\end{matrix}{where}H_{k}} = \begin{Bmatrix}{{{- g} \cdot \cos}\vartheta_{k}} & 0 \\{{{- g} \cdot \sin}\phi_{k}\cos\vartheta_{k}} & {{g \cdot \cos}\phi_{k}\cos\vartheta_{k}}\end{Bmatrix}} \right. & (5)\end{matrix}$

is a measurement matrix at time k, g is a local gravity acceleration,ϕ_(k) and

_(k) are one-step prediction values of a horizontal attitude at time k,K_(k) is a filtering gain at time k, {circumflex over (X)}_(k) is astate estimation at time k, and P_(k) is a state estimation mean squareerror matrix at time k.

In step 10, an estimated value of the state quantity at the k^(th) timeis taken as an initial value of a state quantity at a next time, andsteps 6 to 9 are repeatedly performed.

The self-adaptive horizontal attitude measurement method based on motionstate monitoring has been completed so far.

In order to illustrate the effectiveness of an algorithm, the algorithmis simulated. Simulation conditions are set as follows. A zero offset ofa gyroscope of each axis is set to 1°/h, and a zero offset of anaccelerometer is set to 1×10⁻⁴ g. A motion state is set as follows. Acarrier is in an oscillating motion state. The oscillation of each axisfollows the sine function law. Oscillation amplitudes of rolling,pitching, and heading are all 10°, oscillation periods are all 10^(S),and initial attitude angles and phase angles are all 0°. The simulationresults are shown in FIG. 3. The horizontal attitude measurement erroris small, and the precision is high.

Although examples and drawings of the disclosure have been disclosed forillustrative purposes, those skilled in the art will appreciate that:various substitutions, changes and modifications are possible withoutdeparting from the spirit and scope of the disclosure and the appendedclaims, and therefore the scope of the disclosure is not limited to thedisclosure of the examples and drawings.

What is claimed is:
 1. A self-adaptive horizontal attitude measurementmethod based on motion state monitoring, comprising the following steps:step 1: initially aligning a strapdown inertial navigation system,completing the calculation of a random constant zero offset of a deviceand the calculation of initial horizontal attitude angles, comprising aroll angle

₀ and a pitch angle ϕ₀ , and then entering a navigation working mode;step 2: initializing a Kalman filter, and taking the initial horizontalattitude angles

₀ and ϕ₀ obtained in step 1 as initial values of a Kalman filteringstate quantity X₀=[

₀ ϕ₀]^(T), an initial mean square error being P₀; step 3: sampling microelectro mechanical system inertial measurement unit (MEMS-IMU) data at ak^(th) time, and compensating a random constant zero offset thereof toobtain a compensated specific force f_(k) ^(b) and angular velocityω_(k) ^(b); step 4: performing a Kalman filtering one-step prediction byusing an angular velocity increment Δω_(k) ^(b) at the k^(th) time as aknown deterministic input u_(k−1), wherein Δω_(k) ^(b)=ω_(k) ^(b)·T, Tbeing a calculation period; step 5: judging a maneuvering state of acarrier by using the specific force f^(b) and the angular velocity ω^(b)from time k−N+1 to time k obtained in step 3, and self-adaptivelyadjusting a Kalman filtering measurement noise covariance matrix R_(k),wherein N is the size of a data window; step 6: when the specific forceis f_(k) ^(b)=[f_(x,k) f_(y,k) f_(z,k)]^(T) at the k^(th) time,selecting a measurement vector Z_(k)=[f_(x,k) f_(y,k)]^(T) to performmeasurement update so as to realize the correction of the statequantity, wherein f_(x,k), f_(y,k), and f_(z,k) are components of thespecific force f_(k) ^(b) in x-axis, y-axis, and z-axis directions of acarrier system respectively; and step 7: taking an estimated value ofthe state quantity at the k^(th) time as an initial value of a statequantity at a next time, and repeatedly performing steps 3 to 6 until anavigation working state ends.
 2. The self-adaptive horizontal attitudemeasurement method based on motion state monitoring according to claim1, wherein the Kalman filtering one-step prediction in step 4 comprises:$\left\{ \begin{matrix}{{\overset{\hat{}}{X}}_{{k/k} - 1} = {{\Phi_{{k/k} - 1}{\overset{\hat{}}{X}}_{k - 1}} + {B_{k - 1}u_{k - 1}}}} \\{P_{{k/k} - 1} = {{\Phi_{{k/k} - 1}P_{k - 1}\Phi_{{k/k} - 1}^{T}} + Q_{k - 1}}}\end{matrix} \right.$ where {circumflex over (X)}_(k−1) is a stateoptimal estimation at a k−1^(th) time, P_(k−), is a mean square errormatrix of a state estimation at the k−1^(th) time, {circumflex over(X)}_(k/k−1) is a state one-step prediction at a k^(th) time,Φ_(k/k−1)=I₂ is a state one-step transition matrix at the k^(th) time,I₂ is a second-order unit matrix, $B_{k} = \begin{bmatrix}0 & {\cos\phi_{k - 1}} & {{- \sin}\phi_{k - 1}} \\1 & {\sin\phi_{k - 1}\tan\vartheta_{k - 1}} & {\cos\phi_{k - 1}\tan\vartheta_{k - 1}}\end{bmatrix}$ is an input coefficient matrix at the k^(th) time,ϕ_(k−1) and

_(k−1) are horizontal attitude optimal estimations at the k−1^(th) time,u_(k−1) is an angular velocity increment Δω_(k) ^(b) at the k^(th) time,P_(k/k−1) is a state one-step prediction mean square error matrix at thek^(th) time, and Q_(k) is a system noise variance matrix.
 3. Theself-adaptive horizontal attitude measurement method based on motionstate monitoring according to claim 1, wherein in step 5, the judging amaneuvering state of a carrier by using the specific force f^(b) and theangular velocity ω^(b) obtained in step 3 comprises: calculating themaneuvering state of the carrier by using the specific force f^(b) andthe angular velocity ω^(b) from time k−N+1 to time k obtained in step 3,and acquiring a maneuvering vector T_(k) to realize maneuveringjudgment, T_(k) satisfying:$T_{k} = \left( {{\frac{1}{\sigma_{f}^{2}}{{f_{k}^{b} - {g\frac{{\overset{¯}{f}}_{k}}{{\overset{¯}{f}}_{k}}}}}} + {\frac{1}{\sigma_{\omega}^{2}}{{\overset{\_}{\omega}}_{k}}}} \right)$${{where}{\overset{¯}{\omega}}_{k}} = {{\frac{1}{N}{\sum\limits_{i = {k - N + 1}}^{k}{\omega_{i}^{b}{and}{\overset{¯}{f}}_{k}}}} = {\frac{1}{N}{\sum\limits_{i = {k - N + 1}}^{k}f_{i}^{b}}}}$are mean values obtained by performing moving average on the specificforce f^(b) and the angular velocity ω^(b) from time k−N+1 to time k instep 3, respectively, the size of a data window being N, g being a localgravitational acceleration, and σ_(f) and σ^(ω)being weightingcoefficients respectively.
 4. The self-adaptive horizontal attitudemeasurement method based on motion state monitoring according to claim3, wherein the self-adaptively adjusting a Kalman filtering measurementnoise covariance matrix R_(k) in step 5 comprises:$R_{k} = \begin{bmatrix}{\sigma_{r}^{2} + T_{k,1}} & 0 \\0 & {\sigma_{r}^{2} + T_{k,2}}\end{bmatrix}$ where T_(k,i) is an i^(th) element of T_(k), and σ_(r) ²is a variance corresponding to an accelerometer noise.
 5. Theself-adaptive horizontal attitude measurement method based on motionstate monitoring according to claim 1, wherein an update equation of themeasurement update in step 6 is: $\left\{ {{\begin{matrix}{K_{k} = {P_{{k/k} - 1}{H_{k}^{T}\left( {{H_{k}P_{{k/k} - 1}H_{k}^{T}} + R_{k}} \right)}^{- 1}}} \\{{\overset{\hat{}}{X}}_{k} = {{\overset{\hat{}}{X}}_{{k/k} - 1} + {K_{k}\left( {Z_{k} - {H_{k}{\overset{\hat{}}{X}}_{{k/k} - 1}}} \right)}}} \\{P_{k} = {\left( {I - {K_{k}H_{k}}} \right)P_{{k/k} - 1}}}\end{matrix}{where}H_{k}} = \begin{Bmatrix}{{{- g} \cdot \cos}\vartheta_{k}} & 0 \\{{{- g} \cdot \sin}\phi_{k}\cos\vartheta_{k}} & {{g \cdot \cos}\phi_{k}\cos\vartheta_{k}}\end{Bmatrix}} \right.$ is a measurement matrix at time k, g is a localgravity acceleration, ϕ_(k) and

_(k) are one-step prediction values of a horizontal attitude at time k,K_(k) is a filtering gain at time k, {circumflex over (X)}_(k) is astate estimation at time k, and P_(k) is a state estimation mean squareerror matrix at time k.